#include "StatisticalOutlierRemover.h"
#include "dialogs/SORDialog.h"

#include <cc2sm.h>
#include <filtering.h>
#include <my_point_types.h>
#include <sm2cc.h>

#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/statistical_outlier_removal.h>

StatisticalOutlierRemover::StatisticalOutlierRemover()
	: BaseFilter(FilterDescription("Statistical Outlier Remover", "Remove Outliers Using statistical Approach", "Remove Outliers out of a given distance from the point, expressed as sigma of mean distances", ":/toolbar/PclUtils/icons/sor_outlier_remover.png", true))
	, m_dialog(0)
	, knn(0)
	, stddev(0)
{

}

StatisticalOutlierRemover::~StatisticalOutlierRemover()
{
	if (m_dialog)
		delete m_dialog;
}

int StatisticalOutlierRemover::openDialog()
{
	if (!m_dialog)
	{
		m_dialog = new SORDialog;
	}

	ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();

	return m_dialog->exec() ? 1 : 0;
}

void StatisticalOutlierRemover::getParametersFromDialog()
{
	assert(m_dialog);
	if (!m_dialog)
		return;

    //fill in parameters from dialog
   
    knn = m_dialog->knnSpinBox->value();
	stddev = m_dialog->stdDevSpinBox->value();
}

int StatisticalOutlierRemover::compute()
{
	ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
	if (!cloud)
		return -1;

	sensor_msgs::PointCloud2::Ptr sm_cloud (new sensor_msgs::PointCloud2);
	
	//get xyz in sensor_msgs format
    cc2smReader converter;
    converter.setInputCloud(cloud);
	converter.getAsSM(*sm_cloud);


	//pcl::StatisticalOutlierRemoval<sensor_msgs::PointCloud2> sor;
	//sor.setInputCloud(sm_cloud);
	//sor.setMeanK(knn);
	//sor.setStddevMulThresh(stddev);

	sensor_msgs::PointCloud2::Ptr out_sm_cloud (new sensor_msgs::PointCloud2);
	//sor.filter(*out_sm_cloud);
	remove_outliers(sm_cloud,out_sm_cloud,knn,stddev);
	boost::shared_ptr<sensor_msgs::PointCloud2> cloud_ptr = boost::make_shared<sensor_msgs::PointCloud2>(*out_sm_cloud);
	
	sm2ccReader reader;
	ccPointCloud* out_c = new ccPointCloud;
	reader.setInputCloud(cloud_ptr);
	int good = reader.getAsCC(out_c);
	if(good!=1){
		delete out_c;
		return -1;

	}
	ccHObject* cloud_container = new ccHObject();

	cloud_container->addChild(out_c);
	cloud_container->setName(qPrintable("SOR"));

	emit newEntity(cloud_container);

	return 1;
}

